#include "io430x22x4.h"
#include "intrinsics.h"
#include "cap.h"
#include "adc.h"
#include "comparator.h"
#include "sem_core.h"


void _update_vc_initial_new_cap_new_partial(float v_adc);
void _update_vc_initial_new_partial(float v_adc);
void _discharged_capacitor(void);
void _get_partial_metering(float v_adc);

//---------------  Constantes  ----------------
#define NUMBER_OF_C 2  // numero de capacitores
#define V_TH 2.2       // Voltaje de unmbral de descarga
//---------------  Variables  ----------------
float vc_initial_new_cap;       // Voltaje inicial del capacitor al cambiarlo
float vc_initial_new_partial;   // Voltaje inicial de medida de consumo parcial

float q_total;          // Q (J=F*V) desde el inicio del sistema. 
float q_partial;        // Q parcial
int c_active;         // numero de capacitor activo
float c_value[NUMBER_OF_C] = { 0.88, 0.82 }; // (mF) array con los valores de los capacitores

float vc_arranque = 0;

static void (* partial_metering_registered_callback_fn)(float) ;

//---------------  funciones auxiliares   ----------------
void _update_vc_initial_new_cap_new_partial(float v_adc){
  // invocada cuando ADC termina conversion al resetar sistema 
  //  o luego de cambiar de capacitor
  vc_initial_new_cap= v_adc;
  vc_initial_new_partial= v_adc;
}

void _update_vc_initial_new_partial(float v_adc){
  // invocada cuando ADC termina conversion al 
  //  o luego de cambiar de capacitor
  vc_initial_new_partial = v_adc;
}

void _discharged_capacitor(){
  // invocada por comparador cuando se llego a Vth y hay que cambiar de capacitor

  //  * actualizar q_total = c_value[c_active] * (vc_initial_new_cap - vth)
  q_total += c_value[c_active-1] * (vc_initial_new_cap - V_TH);

  //  * cambiar de capacitor (en hw y variable c_active)
  c_active++;
  if (c_active > NUMBER_OF_C) {
    c_active= 1;
  }
  cap_sel(c_active);

  //  * disparar conversion para que actualice v_initial
 adc_start(_update_vc_initial_new_cap_new_partial);
  
}

void _get_partial_metering(float v_adc){
  q_partial += c_value[c_active-1] * (vc_initial_new_partial - v_adc); // actualizo q parcial
  vc_initial_new_partial= v_adc;// actualizo vc para nuevo calculo de parcial
  partial_metering_registered_callback_fn(q_partial);
}


//--------------- inicializaciones y resets  ----------------
void sem_init(){
  adc_init();           // inicializo ADC y vref
  comparator_init(_discharged_capacitor);   // inicializacion de comparador, registrando _discharged_capacitor()
  cap_init();  // inicializo condensadores en modo carga y bypass

  // espero y selecciono un capacitor (para cargar el capacitor?)

  __delay_cycles(4000000);
  
//  for (long i=0 ; i<=5000000; i++); //delay
//  vc_arranque = adc_start_block();
//  while (vc_arranque < 2.8) {
//    vc_arranque = adc_start_block();
//    for (long i=0 ; i<=50000; i++); //delay
//  };
  
  c_active= 1;
  cap_sel(c_active);

  sem_reset_global_meter();   // luego invoco reset global
}

void sem_reset_global_meter(){
  q_total= 0;
  adc_start(_update_vc_initial_new_cap_new_partial);  // invoca adc para que actualice  vc_initial_new_cap = lectura de ADC
}

//---------------- consumo global -------------------------
float sem_get_global_meter(){
  return q_total;
}

//---------------- consumo parcial -------------------------
void sem_start_partial_metering(){
  q_partial = 0;
 // invoca adc para que actualice  vc_initial_new_partial = lectura de ADC
  adc_start(_update_vc_initial_new_partial);
}


void sem_get_partial_metering( void (*callback_fn)(float) ){
  partial_metering_registered_callback_fn= callback_fn; 
  adc_start(_get_partial_metering);  

 // invoca adc para que actualice  vc_initial_new_partial = lectura de ADC
  //  adc_start(_update_vc_initial_new_partial);
}
